#!/usr/bin/python3
import yaml
import rospy
import numpy as np
from std_srvs.srv import *
from village.srv import *
import math
from numpy import array
from numpy import mat
class Region:  	

	village = {}
	def __init__(self):
		self.village['id1'] = rospy.get_param("/id1")
		self.village['id2'] = rospy.get_param("/id2")
		self.village['id3'] = rospy.get_param("/id3")
		self.village['id4'] = rospy.get_param("/id4")
		self.village['id5'] = rospy.get_param("/id5")
		self.village['id6'] = rospy.get_param("/id6")
		self.village['id7'] = rospy.get_param("/id7")

	def get_region(self):
		return self.village
		
	def get_village(self,id):
		return self.village[id]
		
	def set_village(self,id, x, y):
		self.village[id]['x'] = x		
		self.village[id]['y'] = y
		return self.village
		
	def add_village(self,id, x, y):
		self.village.update({id: {'x':x,'y':y}})
		return self.village
	"删除有bug，不能实现最小生成树"	
	def del_village(self,id):
		del self.village[id]
	
	def min_dist(self,id1,id2):
	
		lenth = len(self.get_region())
		arr = [[0]*(lenth+1) for _ in range(lenth+1)]
		for i in range(1,lenth+1):
			for j in range(1,lenth+1):
				arr[i][j] = GetDist(i,j)

		r_mat=mat(arr)
		np.set_printoptions(linewidth=400)
		rospy.loginfo("邻接矩阵:")
		print(r_mat)
		
		path = [[0]*(lenth+1) for _ in range(lenth+1)]
		dis = [[0]*(lenth+1) for _ in range(lenth+1)]

		for v in range(1,lenth+1):
			for w in range(1,lenth+1):
				dis[v][w] = arr[v][w]
				path[v][w] = w

		for k in range(1,lenth+1): 
			for v in range(1,lenth+1):
				for w in range(1,lenth+1):
					if dis[v][w] > dis[v][k] + dis[k][w]:
						dis[v][w] = dis[v][k] + dis[k][w]
						path[v][w] = path[v][k]

		rospy.loginfo("dis[][]:")
		
		r_mat1=mat(dis)
		np.set_printoptions(linewidth=400)
		print(r_mat1)
		
		rospy.loginfo("path[][]:")
		r_mat2=mat(path)
		np.set_printoptions(linewidth=400)
		print(r_mat2)

		rospy.loginfo("已调用min_dist():")
		rospy.loginfo("节点%d->%d的最短路程:%f",id1,id2,dis[id1][id2])
		k = path[id1][id2]
		print("path:%d" % id1, end = "")
		while(k != id2):
			print("->%d"% k, end = "")
			k = path[k][id2]
		print("->%d"%id2)
		
	def min_dist_villages(self,num):
	
		lenth = len(self.get_region())
		global infinity
		arr = [[0]*(lenth+1) for _ in range(lenth+1)]
		for i in range(1,lenth+1):
			for j in range(1,lenth+1):
				arr[i][j] = GetDist(i,j)
		rospy.loginfo("邻接矩阵:")
		r_mat=mat(arr)
		np.set_printoptions(linewidth=400)
		print(r_mat)
		
		flag = [0]*20
		dis = [0]*20

		for i in range(1,lenth+1):
			dis[i]=arr[num][i]
		flag[num]=1
		count=1
		j=num
		sum=0
		
		rospy.loginfo("最小生成树:")
		while count < lenth:  
			min = infinity
			k = j
			for i in range(1,lenth+1):
				if (flag[i]==0 and dis[i]<min):
					min = dis[i]
					j = i	
			rospy.loginfo("id%d-->id%d = %f",k,j,min)
	    
			flag[j] = 1
			count += 1
			sum += dis[j]
			for i in range(1,lenth+1):
				if (flag[i]==0 and arr[j][i] < dis[i]):
					dis[i] = arr[j][i]
		rospy.loginfo("权值之和 = %f",sum)
	
	
def get_region(self): 
	rospy.loginfo("已调用get_region():")
	print(village1.get_region())
	response = EmptyResponse()
	return response
	
def get_village(request): 
	rospy.loginfo("已调用get_village():")
	id = request.id
	print(village1.get_village(id))
	response = getResponse()
	return response
	
def set_village(request): 
	global num
	rospy.loginfo("已调用set_village():")
	id = request.id
	x = request.x
	y = request.y
	list1 = list(id)
	lenth = len(village1.get_region())
	response = VILLAGE_setResponse()
	if int(list1[2]) <= lenth:
		a = village1.set_village(id,x,y)
		response.x1 = a[id]['x']
		response.y1 = a[id]['y']
		rospy.loginfo("%s重置后的坐标：(%d,%d)",id,response.x1,response.y1)
		village1.min_dist_villages(1)
	else:rospy.loginfo("没有该坐标，设置失败！")
	return response
	
def add_village(request): 
	rospy.loginfo("已调用add_village():")
	id = request.id
	x = request.x
	y = request.y
	list1 =list(id)
	
	lenth = len(village1.get_region())
	response = VILLAGE_setResponse()
	if int(list1[2]) > lenth:
		a = village1.add_village(id,x,y)
		response.x1 = a[id]['x']
		response.y1 = a[id]['y']
		rospy.loginfo("%s新增的坐标：(%d,%d)",id,response.x1,response.y1)
		village1.min_dist_villages(1)
	else:rospy.loginfo("该坐标已经存在，增加失败！")
	return response

def del_village(request): 
	rospy.loginfo("已调用del_village():")
	id = request.id
	response = getResponse()
	village1.del_village(id)
	rospy.loginfo("已删除%s",id)
	village1.min_dist_villages(1)
	return response

def min_dist1(request):
	id1 = request.id1
	id2 = request.id2
	village1.min_dist(id1,id2)
	response = floydResponse()
	return response
	
def min_dist_villages(request): 
	rospy.loginfo("已调用get_village():")
	num = request.num
	village1.min_dist_villages(num)
	response = numResponse()
	return response
	
def GetDist(id1, id2):
	id11 = 'id'+str(id1)
	id22 = 'id'+str(id2)
	id11 = village1.get_village(id11)
	id22 = village1.get_village(id22)
	dis = math.sqrt((id11['x'] - id22['x']) ** 2 + (id11['y'] - id22['y']) ** 2)
	return dis
	


if __name__ == "__main__":
	
	infinity = 65535 
	village1 = Region()
		
	rospy.init_node("fuwu")
	server = rospy.Service("get_region",Empty,get_region)
	server = rospy.Service("get_village",get,get_village)
	server = rospy.Service("set_village",VILLAGE_set,set_village)
	server = rospy.Service("add_village",VILLAGE_set,add_village)
	server = rospy.Service("del_village",get,del_village)
	server = rospy.Service("min_dist",floyd,min_dist1)
	server = rospy.Service("min_dist_village",num,min_dist_villages)
	rospy.loginfo("服务器已经启动：")
	
	rospy.spin()
	



	
	
	
	
